package edu.wpi.first.wpilibj.frc_2011;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.camera.*;
/**
 * Main robot control
 * @author Daniel Flaws
 */
public class FRC_2011 extends SimpleRobot {
    
    RobotDrive driveTrain;
    DriverStationLCD dsLCD;
    AxisCamera cam;
    Joystick leftStick;
    static Joystick rightStick;
    Joystick control;
    Servo xCam;
    Servo yCam;
    DigitalInput left;
    DigitalInput middle;
    DigitalInput right;
    Jaguar claw;
    Servo release;
    static Jaguar winch1;
    static Jaguar winch2;
    Jaguar feed;
    static Gyro gyro;
    double oldZ;
    ArmMoveUp moveUp = new ArmMoveUp();
    ArmMoveDown moveDown = new ArmMoveDown();
    DriverStation ds;
    DigitalInput touch;
    Compressor comp;
    Solenoid one, two;
    public FRC_2011() {
	driveTrain = new RobotDrive(1,2,3,4);
        dsLCD = DriverStationLCD.getInstance();
	leftStick = new Joystick(1);
	rightStick = new Joystick(2);
        control = new Joystick(3);
        cam = AxisCamera.getInstance();
        xCam = new Servo(5);
        yCam = new Servo(6);
        left = new DigitalInput(1);
        middle = new DigitalInput(2);
        right = new DigitalInput(3);
        claw = new Jaguar(6, 1);
        winch1 = new Jaguar(7);
        winch2 = new Jaguar(8);
        release = new Servo(9);
        feed = new Jaguar(6, 2);
        gyro = new Gyro(1);
        ds = DriverStation.getInstance();
        comp = new Compressor(4,1);
        touch = new DigitalInput(5);
        one = new Solenoid(1);
        two = new Solenoid(2);
    }
    boolean reset = false;
    public void resetCam() {
        if (!reset) {
            xCam.set(0.5);
            yCam.set(0.5);
            reset = true;
        }
    }
    public void robotInit() {
        cam.writeCompression(0);
        cam.writeResolution(AxisCamera.ResolutionT.k640x480);
    }
    public void disabled() {
        gyro.reset();
        dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Disabled  ");
        dsLCD.updateLCD();
        resetCam();
        comp.stop();
    }
    
    double defaultSteeringGain = 0.65;
    public void autonomous() {

        int binaryValue; // a single binary value of the three line tracking
                        // sensors
        int previousValue = 0; // the binary value from the previous loop
        double steeringGain; // the amount of steering correction to apply

        // the power profiles for the straight and forked robot path. They are
        // different to let the robot drive more slowly as the robot approaches
        // the fork on the forked line case.


        double forkProfile[] = {0.70, 0.70, 0.55, 0.60, 0.60, 0.50, 0.40, 0.00};
        double straightProfile[] = {0.7, 0.7, 0.6, 0.6, 0.35, 0.35, 0.35, 0.0};

        double powerProfile[];   // the selected power profile

        // set the straightLine and left-right variables depending on chosen path
        boolean straightLine = ds.getDigitalIn(1);
        powerProfile = (straightLine) ? straightProfile : forkProfile;
        double stopTime = (straightLine) ? 2.0 : 4.0; // when the robot should look for end
        boolean goLeft = !ds.getDigitalIn(2) && !straightLine;
        System.out.println("StraightLine: " + straightLine);
        System.out.println("GoingLeft: " + goLeft);


        boolean atCross = false; // if robot has arrived at end

        // time the path over the line
        Timer timer = new Timer();
        timer.start();
        timer.reset();

        int oldTimeInSeconds = -1;
        double time;
        double speed, turn;

        // loop until robot reaches "T" at end or 8 seconds has past
        while ((time = timer.get()) < 8.0 && !atCross) {
            int timeInSeconds = (int) time;
            // read the sensors
            int leftValue = !left.get() ? 1 : 0;
            int middleValue = !middle.get() ? 1 : 0;
            int rightValue = !right.get() ? 1 : 0;
            // compute the single value from the 3 sensors. Notice that the bits
            // for the outside sensors are flipped depending on left or right
            // fork. Also the sign of the steering direction is different for left/right.
            if (goLeft) {
                binaryValue = leftValue * 4 + middleValue * 2 + rightValue;
                steeringGain = -defaultSteeringGain;
            } else {
                binaryValue = rightValue * 4 + middleValue * 2 + leftValue;
                steeringGain = defaultSteeringGain;
            }

            // get the default speed and turn rate at this time
            speed = powerProfile[timeInSeconds];
            turn = 0;

            // different cases for different line tracking sensor readings
            switch (binaryValue) {
                case 1:  // on line edge
                    turn = 0;
                    break;
                case 7:  // all sensors on (maybe at cross)
                    if (time > stopTime) {
                        atCross = true;
                        speed = 0;
                    }
                    break;
                case 0:  // all sensors off
                    if (previousValue == 0 || previousValue == 1) {
                        turn = steeringGain;
                    } else {
                        turn = -steeringGain;
                    }
                    break;
                default:  // all other cases
                    turn = -steeringGain;
            }
            // print current status for debugging
            if (binaryValue != previousValue) {
                System.out.println("Time: " + time + " Sensor: " + binaryValue + " speed: " + speed + " turn: " + turn + " atCross: " + atCross);
            }

            // set the robot speed and direction
            driveTrain.arcadeDrive(speed, turn);

            if (binaryValue != 0) {
                previousValue = binaryValue;
            }
            oldTimeInSeconds = timeInSeconds;

            Timer.delay(0.01);
        }
        // Done with loop - stop the robot. Robot ought to be at the end of the line
        driveTrain.arcadeDrive(0, 0);
    }
    double feedSpeed = 0;
    public void operatorControl() {
        dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Teleop    ");
        dsLCD.updateLCD();
        comp.start();
        oldZ = rightStick.getRawAxis(3);
        gyro.reset();
        while(isOperatorControl() && isEnabled()) {
            double leftY = leftStick.getRawAxis(2) * .5;
            double rightY = rightStick.getRawAxis(2) * .5;
            if(!rightStick.getRawButton(1))
                driveTrain.tankDrive(leftStick,rightStick);
            else
                driveTrain.tankDrive(leftY, rightY);

            dsLCD.println(DriverStationLCD.Line.kUser3, 1, "" + getGyroAngle());
            SmartDashboard.log(getGyroAngle(), "Gyro");
            dsLCD.updateLCD();

            if(leftStick.getRawAxis(3) < 0) {
                if(leftStick.getRawButton(4)) {
                    if (xCam.get() > 0) {
                        xCam.set(xCam.get() - .005);
                        reset = false;
                    }
                }
                if (leftStick.getRawButton(5)) {
                    if (xCam.get() < 1) {
                        xCam.set(xCam.get() + .005);
                        reset = false;
                    }
                }
                if (leftStick.getRawButton(3)) {
                    if (yCam.get() < 1) {
                        yCam.set(yCam.get() + .005);
                        reset = false;
                    }
                }
                if (leftStick.getRawButton(2)) {
                    if (yCam.get() > 0) {
                        yCam.set(yCam.get() - .005);
                        reset = false;
                    }
                }
            } else {
                if(leftStick.getRawButton(4)) {
                    xCam.set(.25);
                    reset = false;
                } else if (leftStick.getRawButton(5)) {
                    xCam.set(.75);
                    reset = false;
                } else if (leftStick.getRawButton(3)) {
                    yCam.set(.75);
                    reset = false;
                } else if (leftStick.getRawButton(2)) {
                    yCam.set(.25);
                    reset = false;
                } else
                    resetCam();
            }
            if(leftStick.getRawButton(1))
                resetCam();
            if (control.getRawButton(10))
                release.set(1);
            else
                release.set(0);
            if (control.getRawButton(6))
                claw.set(.7);
            else if (control.getRawButton(8))
                claw.set(.3);
            else
                claw.set(0);
            winch1.set(control.getRawAxis(2));
            winch2.set(control.getRawAxis(2));
            if(control.getRawButton(5))
                feedSpeed = .3;
            else if(control.getRawButton(7))
                feedSpeed = -.3;
            else if(control.getRawButton(9))
                feedSpeed = 0;
            feedSpeed = (touch.get()) ? feedSpeed : -feedSpeed;
            feed.set(feedSpeed);
            if (control.getRawButton(4)) {
                one.set(true);
                two.set(false);
            } else if (control.getRawButton(2)) {
                one.set(false);
                two.set(true);
            }
/**            double rightZ = rightStick.getRawAxis(3);
            if (rightZ > oldZ)
                new Thread(moveUp).start();
            else if (rightZ < oldZ)
                new Thread(moveDown).start();
            oldZ = rightZ;*/
        }
    }
    public static double getGyroAngle() {
        double angle = gyro.getAngle() % 360;
        if (angle > 180) {
            angle = -(angle - 360);
        }
        return angle;
    }
}